#include "UniformGrid.h"

#include <algorithm>
#include <cmath>

namespace physicsworks {

using boost::dynamic_bitset;

UniformGrid::UniformGrid(const vec3& a, const vec3& b, real cellDiameter, int numBuckets)
 : cells(numBuckets), cellDiameter(cellDiameter)
{
	max.x = std::max(a.x, b.x);
	max.y = std::max(a.y, b.y);
	max.z = std::max(a.z, b.z);
	
	min.x = std::min(a.x, b.x);
	min.y = std::min(a.y, b.y);
	min.z = std::min(a.z, b.z);
	
	numCellsX = static_cast<int>( ceil((max.x - min.x) / cellDiameter) );
	numCellsY = static_cast<int>( ceil((max.x - min.x) / cellDiameter) );
	numCellsZ = static_cast<int>( ceil((max.x - min.x) / cellDiameter) );
	
	occupiedCells.resize(numCellsX * numCellsY * numCellsZ);
}

UniformGrid::~UniformGrid()
{

}

list<pair<RigidBody*,RigidBody*> > UniformGrid::getPotentialCollidingPairs()
{
	list<pair<RigidBody*,RigidBody*> > pl;
	
	list<CellCoordinates>::iterator citer;
	for (citer = occupiedCellsList.begin(); citer != occupiedCellsList.end(); ++citer) {
		
		const list<RigidBodyMapping*>& rbl = cells[*citer];
		list<RigidBodyMapping*>::const_iterator riter0, riter1;
		for (riter0 = rbl.begin(); riter0 != rbl.end(); ++riter0) {
			
			// assert the body is inside the cell *citer
			if ( (*riter0)->cellCoordinates == *citer ) {
			
				riter1 = riter0;
				findPotentialCollidingPairs(pl, (*riter0)->body, ++riter1, rbl.end(), *citer);
				
				const list<RigidBodyMapping*>* rblt;
				AABox box = (*riter0)->body->getBoundingBox();
				bool hitEast = false;
				bool hitSouth = false;
				 
				if (citer->getEast() == generateCellCoordinates(vec3(box.min.x, box.max.y, box.max.z))) {
					
					hitEast = true;
					
					rblt = &cells[citer->getEast()];
					findPotentialCollidingPairs(pl, (*riter0)->body, rblt->begin(), rblt->end(), citer->getEast());
					
					if (citer->getSouth() == generateCellCoordinates(vec3(box.min.x, box.min.y, box.min.z))) {
						
						hitSouth = true;
						
						rblt = &cells[citer->getSouth()];
						findPotentialCollidingPairs(pl, (*riter0)->body, rblt->begin(), rblt->end(), citer->getSouth());
						
						rblt = &cells[citer->getSouthEast()];
						findPotentialCollidingPairs(pl, (*riter0)->body, rblt->begin(), rblt->end(), citer->getSouthEast());
						
						rblt = &cells[citer->getSouthWest()];
						findPotentialCollidingPairs(pl, (*riter0)->body, rblt->begin(), rblt->end(), citer->getSouthWest());
					}
				} else if (citer->getSouth() == generateCellCoordinates(vec3(box.min.x, box.min.y, box.min.z))) {
					
					hitSouth = true;
					
					rblt = &cells[citer->getSouth()];
					findPotentialCollidingPairs(pl, (*riter0)->body, rblt->begin(), rblt->end(), citer->getSouth());
					
					rblt = &cells[citer->getSouthWest()];
					findPotentialCollidingPairs(pl, (*riter0)->body, rblt->begin(), rblt->end(), citer->getSouthWest());
				}
				
				if (citer->getFrontCenter() == generateCellCoordinates(vec3(box.max.x, box.min.y, box.max.z))) {
					
					rblt = &cells[citer->getFrontCenter()];
					findPotentialCollidingPairs(pl, (*riter0)->body, rblt->begin(), rblt->end(), citer->getFrontCenter());
					
					if (hitEast) {
						
						rblt = &cells[citer->getFrontEast()];
						findPotentialCollidingPairs(pl, (*riter0)->body, rblt->begin(), rblt->end(), citer->getFrontEast());
						
						if (hitSouth) {
							
							rblt = &cells[citer->getFrontSouth()];
							findPotentialCollidingPairs(pl, (*riter0)->body, rblt->begin(), rblt->end(), citer->getFrontSouth());
						
							rblt = &cells[citer->getFrontSouthEast()];
							findPotentialCollidingPairs(pl, (*riter0)->body, rblt->begin(), rblt->end(), citer->getFrontSouthEast());
						
							rblt = &cells[citer->getFrontSouthWest()];
							findPotentialCollidingPairs(pl, (*riter0)->body, rblt->begin(), rblt->end(), citer->getFrontSouthWest());
						}
					} else if (hitSouth) {
						
						rblt = &cells[citer->getFrontSouth()];
						findPotentialCollidingPairs(pl, (*riter0)->body, rblt->begin(), rblt->end(), citer->getFrontSouth());
					
						rblt = &cells[citer->getFrontSouthWest()];
						findPotentialCollidingPairs(pl, (*riter0)->body, rblt->begin(), rblt->end(), citer->getFrontSouthWest());
					}
				}
				
			}
		}

	}
	
	return pl;
}

/**
 * \param pl List of potential colliding pairs.
 * \param body The left body to be inserted into the list.
 * \param beginIter First body of the cell.
 * \param endIter End iterator of the cell.
 * \param cc CellCoordinates of that cell.
 */
void UniformGrid::findPotentialCollidingPairs(list<pair<RigidBody*,RigidBody*> >& pl, RigidBody* body,
                                              list<RigidBodyMapping*>::const_iterator beginIter,
                                              list<RigidBodyMapping*>::const_iterator endIter, const CellCoordinates& cc)
{
	while (beginIter != endIter) {
		if ( (*beginIter)->cellCoordinates == cc ) {// is the body inside the cell *citer?
			
			pl.push_back( pair<RigidBody*,RigidBody*>( body, (*beginIter)->body) );
		}
		++beginIter;		
	}
}

/**
 * Insert the rigid body into the rigidBodies list.
 */
void UniformGrid::insert(RigidBody* b)
{
	AABox box = b->getBoundingBox();
	CellCoordinates c = generateCellCoordinates(vec3(box.min.x, box.min.y, box.max.z));

	list<RigidBodyMapping*>* listPtr = &cells[c];
	RigidBodyMapping* m = new RigidBodyMapping(listPtr, listPtr->begin(), c, b);
	listPtr->push_back(m);
	rigidBodies[b] = m;
	
	dynamic_bitset<>::reference ref = getOccupiedCellRef(c);
	
	if ( !ref ) {
		ref = true;
		occupiedCellsList.push_back(c);
	}
}

// TODO: assert that the bouding box's radius is permitted
void UniformGrid::add(RigidBody* b)
{
	AABox box = b->getBoundingBox();
	
	if (isInsideGridBoundaries(box)) {
	
		insert(b);
		
	} else {
		
		outRigidBodies.insert(b);
	}
}

void UniformGrid::remove(RigidBody* b)
{
	// the occupiedCells may report false trues after this function
	// but its ok
	
	if (rigidBodies.find(b) != rigidBodies.end()) {
	
		RigidBodyMapping* m = rigidBodies[b];
		m->listPtr->erase(m->iterator);
		rigidBodies.erase(b);
		delete m;
	} else {
		outRigidBodies.erase(b);
	}
}

void UniformGrid::update()
{
	list<RigidBody*> additionalOutRigidBodies;
	
	RigidBodyHMap::iterator iter;
	for (iter = rigidBodies.begin(); iter != rigidBodies.end();) {

		AABox box = (*iter).first->getBoundingBox();
		
		if (!isInsideGridBoundaries(box)) {
			// it's outside the grid, prepare to add this body in the outRigidBodies
			additionalOutRigidBodies.push_back( (*iter).first );
			(*iter).second->listPtr->erase( (*iter).second->iterator );
			delete (*iter).second;
			rigidBodies.erase( iter++ );
			continue;
		}
		
		CellCoordinates c = generateCellCoordinates(vec3(box.min.x, box.min.y, box.max.z));
		
		if (c != (*iter).second->cellCoordinates) {
			// so the body moved to another cell, lets update this
			// obs: the occupiedCells may report false trues after this
			// but its ok
			(*iter).second->listPtr->erase( (*iter).second->iterator );
			list<RigidBodyMapping*>* listPtr = &cells[c];
			listPtr->push_back( (*iter).second );
			(*iter).second->iterator = listPtr->begin();
			(*iter).second->cellCoordinates = c;
		}
		
		++iter;
	}
	
	// update the rigid bodies outside the grid's boundaries
	RigidBodyHSet::iterator oiter;
	for (oiter = outRigidBodies.begin(); oiter != outRigidBodies.end();) {
		
		AABox box = (*oiter)->getBoundingBox();
	
		if (isInsideGridBoundaries(box)) {

			insert(*oiter);
			outRigidBodies.erase(oiter++);
		} else
			++oiter;
	}
	
	outRigidBodies.insert(additionalOutRigidBodies.begin(), additionalOutRigidBodies.end());
}

}
